/*
 * camera.h
 *
 *  Created on: Oct 7, 2012
 *      Author: robot
 */

#ifndef CAMERA_H_
#define CAMERA_H_

#include <ros/ros.h>
#include <sensor/tag.h>

//#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/nonfree/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor/Snapshot.h>

class Camera
{
	ros::NodeHandle 					nh;

	struct 		tag {
		cv::Mat 					image;
		cv::Mat 					descriptor;
		std::vector<cv::KeyPoint> 	keypoints;
	};

public:
				Camera();													  // Camera constructor.
	void 		display ();													  // Display camera images.
	bool		takeSnapshot 		(sensor::Snapshot::Request   &req,
    								 sensor::Snapshot::Response  &res);		 // Snapshot service

private:
	void		captureImage 		();										  // Capture & process images from cam
	void 		findTags 			(cv::Mat haystack);			  			  // Find tags using SIFT / SURF
	int 		determineColor 		(cv::Mat haystack);
	double 		matchTag			(tag needle, tag haystack);				  // Determine if a tag matches.
	void		loadDescriptor  	();										  // Load SURF feature descriptor
	void 		findSquares		(cv::Mat image);						  // Find squares using Canny edge
	void 		findSquares		(cv::Mat image, float angle, bool identify);  // Find squares using Canny edge

	// Read images from a published topic instead of capture
	void 		joinImageTopic		(ros::NodeHandle nh);					  // Join an image publication topic
	void 		parseImageLeft 	(const sensor_msgs::ImageConstPtr& msg); // Parse image from left publisher
	void 		parseImageRight 	(const sensor_msgs::ImageConstPtr& msg); // Parse image from right publisher

	// Capture images from cameras instead of reading them from a topic
	bool 		openCapture 		();										  // Open a cam capture device

	uint							imageIndex;

	cv::VideoCapture * 				captureLeft;
	cv::VideoCapture * 				captureRight;

	// Tag detection publisher
	ros::Publisher 					tag_publisher;
	sensor::tag 					tagMessage;

	// Snapshot service
	ros::ServiceServer 				snapshot_service;

	// The loaded images
	cv::Mat 						left, right;

	// SURF / SIFT feature detector; descriptor; keypoints; images.
	cv::FlannBasedMatcher 			matcher;
	std::vector<cv::DMatch> 		matches;

	cv::SurfFeatureDetector 		detector;
	cv::SurfDescriptorExtractor 	extractor;

	std::vector<tag>				tags;
};


#endif /* CAMERA_H_ */
